/**
  ******************************************************************************
  * @file    encoder_speed_pos_fdbk.c
  * @author  Motor Control SDK Team, ST Microelectronics
  * @brief   This file provides firmware functions that implement the following features
  *          of the Encoder component of the Motor Control SDK:
  *
  *           - computes & stores average mechanical speed
  *           - computes & stores average mechanical acceleration
  *           - computes & stores  the instantaneous electrical speed
  *           - calculates the rotor electrical and mechanical angle
  *
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2019 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under Ultimate Liberty license
  * SLA0044, the "License"; You may not use this file except in compliance with
  * the License. You may obtain a copy of the License at:
  *                             www.st.com/SLA0044
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "encoder_speed_pos_fdbk.h"

#include "mc_type.h"

/** @addtogroup MCSDK
  * @{
  */

/** @addtogroup SpeednPosFdbk
  * @{
  */

/** @defgroup Encoder Encoder Speed & Position Feedback
  * @brief Quadrature Encoder based Speed & Position Feedback implementation
  *
  * This component is used in applications controlling a motor equipped with a quadrature encoder.
  *
  * This component uses the output of a quadrature encoder to provide a measure of the speed and
  * the position of the rotor of the motor.
  *
  * @todo Document the Encoder Speed & Position Feedback "module".
  *
  * @{
  */

/* Private defines -----------------------------------------------------------*/


/**
  * @brief  It initializes the hardware peripherals (TIMx, GPIO and NVIC)
            required for the speed position sensor management using ENCODER
            sensors.
  * @param  pHandle: handler of the current instance of the encoder component
  * @retval none
  */
__weak void ENC_Init( ENCODER_Handle_t * pHandle )
{

  TIM_TypeDef * TIMx = pHandle->TIMx;
  uint8_t BufferSize;
  uint8_t Index;


#ifdef TIM_CNT_UIFCPY
  LL_TIM_EnableUIFRemap ( TIMx );
#define ENC_MAX_OVERFLOW_NB     ((uint16_t)2048) /* 2^11*/
#else
#define ENC_MAX_OVERFLOW_NB     (1)
#endif
  /* Reset counter */
  LL_TIM_SetCounter ( TIMx, 0 );

  /*Calculations of convenience*/
  pHandle->U32MAXdivPulseNumber = UINT32_MAX / ( uint32_t )( pHandle->PulseNumber );//.PulseNumber = 4096*4 = 16384
  pHandle->SpeedSamplingFreqUnit = pHandle->SpeedSamplingFreqHz * SPEED_UNIT;//中频任务频率 1000 , 1000 * 10

  /* Set IC filter for both channel 1 & 2*/
  LL_TIM_IC_SetFilter(TIMx, LL_TIM_CHANNEL_CH1, ( uint32_t )(pHandle->ICx_Filter << 20));
  LL_TIM_IC_SetFilter(TIMx, LL_TIM_CHANNEL_CH2, ( uint32_t )(pHandle->ICx_Filter << 20));

  LL_TIM_ClearFlag_UPDATE ( TIMx );
  LL_TIM_EnableIT_UPDATE ( TIMx );//clear flag before enable it, good habit. entry the nvic when encoder timer counter cnt is 16384,and get a overflow flag

  /* Enable the counting timer*/
  LL_TIM_EnableCounter ( TIMx );

  /* Erase speed buffer */
  BufferSize = pHandle->SpeedBufferSize;

  for ( Index = 0u; Index < BufferSize; Index++ )
  {
    pHandle->DeltaCapturesBuffer[Index] = 0;//clear encoder buffer
  }
}

/**
* @brief  Clear software FIFO where are "pushed" rotor angle variations captured
*         This function must be called before starting the motor to initialize
*         the speed measurement process.
* @param  pHandle: handler of the current instance of the encoder component
* @retval none
*/
__weak void ENC_Clear( ENCODER_Handle_t * pHandle )
{
  uint8_t Index;
  for ( Index = 0u; Index < pHandle->SpeedBufferSize; Index++ )
  {
    pHandle->DeltaCapturesBuffer[Index] = 0;
  }
  pHandle->SensorIsReliable = true;
}

/**
* @brief  It calculates the rotor electrical and mechanical angle, on the basis
*         of the instantaneous value of the timer counter.
* @param  pHandle: handler of the current instance of the encoder component
* @retval int16_t Measured electrical angle in s16degree format.
*/
__weak int16_t ENC_CalcAngle( ENCODER_Handle_t * pHandle )
{
  int32_t wtemp1;
  int16_t elAngle;  /* s16degree format */
  int16_t mecAngle; /* s16degree format */
  /* PR 52926 We need to keep only the 16 LSB, bit 31 could be at 1 
   if the overflow occurs just after the entry in the High frequency task */
   //获得当前的定时器COUNT值，因为有一个分频，是总的定时器的值和编码器一圈的count值
   //转完一圈，count值为4096*4 = 16384时, wtep1 =(int32_t)uint32max = (int32_t)65536*65536 = 0
   //count值为0~16384，wtemp = 0~2,147,483,647,-2,147,483,648~0
  wtemp1 = ( int32_t )( LL_TIM_GetCounter( pHandle->TIMx ) & 0xffff ) *
           ( int32_t )( pHandle->U32MAXdivPulseNumber );

  /*Computes and stores the rotor mechanical angle*/
  mecAngle = ( int16_t )( wtemp1 / 65536 );//获得机械角度，mecAngle一圈为0~32767，-32768~0

  int16_t hMecAnglePrev = pHandle->_Super.hMecAngle;//将上一次的机械角度存起来

  pHandle->_Super.hMecAngle = mecAngle;//本次机械角度存入
  
  /*Computes and stores the rotor electrical angle*/
  elAngle = mecAngle * pHandle->_Super.bElToMecRatio;//获得电气角度 = 机械角度*极对数,为极对数次[0~32767，-32768~0]

  pHandle->_Super.hElAngle = elAngle;//存入电气角度
  //注意此处直接用int16_t,如果是-32768 - 32767=(int16_t)-65535 = 1
  int16_t hMecSpeedDpp = mecAngle - hMecAnglePrev;//获得机械角度的差值，
  pHandle->_Super.wMecAngle += (int32_t)(hMecSpeedDpp);//获得机械角度的差值累计

  /*Returns rotor electrical angle*/
  return ( elAngle );
}

/**
  * @brief  This method must be called with the periodicity defined by parameter
  *         hSpeedSamplingFreqUnit. The method generates a capture event on
  *         a channel, computes & stores average mechanical speed [expressed in the unit
  *         defined by #SPEED_UNIT] (on the basis of the buffer filled by CCx IRQ),
  *         computes & stores average mechanical acceleration [#SPEED_UNIT/SpeedSamplingFreq],
  *         computes & stores the instantaneous electrical speed [dpp], updates the index of the
  *         speed buffer, then checks & stores & returns the reliability state
  *         of the sensor.
  * @param  pHandle: handler of the current instance of the encoder component
  * @param  pMecSpeedUnit pointer used to return the rotor average mechanical speed
  *         (expressed in the unit defined by #SPEED_UNIT)
  * @retval true = sensor information is reliable
  *         false = sensor information is not reliable
  */
__weak bool ENC_CalcAvrgMecSpeedUnit( ENCODER_Handle_t * pHandle, int16_t * pMecSpeedUnit )
{
  TIM_TypeDef * TIMx = pHandle->TIMx;
  int32_t wOverallAngleVariation = 0;
  int32_t wtemp1;
  int32_t wtemp2;
  uint8_t bBufferIndex = 0u;
  bool bReliability = true;
  uint8_t bBufferSize = pHandle->SpeedBufferSize;
  uint32_t OverflowCntSample;
  uint32_t CntCapture;
  uint32_t directionSample;
  uint8_t OFbit = 0;

#ifdef TIM_CNT_UIFCPY
  /* disable Interrupt generation */
  LL_TIM_DisableIT_UPDATE ( TIMx );
#endif
  CntCapture =  LL_TIM_GetCounter ( TIMx );//读取当前定时器的值
  OverflowCntSample = pHandle->TimerOverflowNb;//读取定时器是否溢出，在编码器定时器中断中会有此溢出标志位+1
  pHandle->TimerOverflowNb = 0;//重置溢出标志位
  directionSample =  LL_TIM_GetDirection( TIMx );//获取电机旋转方向
#ifdef TIM_CNT_UIFCPY
  OFbit = __LL_TIM_GETFLAG_UIFCPY( CntCapture );
  if ( OFbit )
  {
    /* If OFbit is set, overflow has occured since IT has been disabled.
    We have to take this overflow into account in the angle computation,
    but we must not take it into account a second time in the accmulator,
    so we have to clear the pending flag. If the OFbit is not set, it does not mean
    that an Interrupt has not occured since the last read, but it has not been taken
    into accout, we must not clear the interrupt in order to accumulate it */
    LL_TIM_ClearFlag_UPDATE( TIMx );
  }
  LL_TIM_EnableIT_UPDATE ( TIMx );
  CLEAR_BIT( CntCapture, TIM_CNT_UIFCPY );
#endif
  /* If UIFCPY is not present, OverflowCntSample can not be used safely for
  speed computation, but we still use it to check that we do not exceed one overflow
  (sample frequency not less than mechanical motor speed */
  if ( ( OverflowCntSample + OFbit ) > ENC_MAX_OVERFLOW_NB )//ENC_MAX_OVERFLOW_NB = 1
  {
    pHandle->TimerOverflowError = true;
  }

  /*Calculation of delta angle*/
  if ( directionSample == LL_TIM_COUNTERDIRECTION_DOWN )//如果是反转，得出差值为负
  {
    /* encoder timer down-counting*/
    /* if UIFCPY not present Overflow counter can not be safely used -> limitation to 1 OF. */
#ifndef TIM_CNT_UIFCPY
    OverflowCntSample = ( CntCapture > pHandle->PreviousCapture ) ? 1 : 0;//当前值大于上一次的读取值，则为1，发生溢出。
#endif
    //CountDirectionDown  16384~0-16384~0,如果OverflowCntSample == 1说明过是0到16384的跳变点 
    //把两次读取的数值的差值存入buffer中，如果有溢出，减去一圈的值 4096*4-1
    pHandle->DeltaCapturesBuffer[pHandle->DeltaCapturesIndex] =
      ( int32_t )( CntCapture ) - ( int32_t )( pHandle->PreviousCapture ) -
      ( ( int32_t )( OverflowCntSample ) + OFbit ) * ( int32_t )( pHandle->PulseNumber );
  }
  else//如果是正转，得出差值为正
  {
    /* encoder timer up-counting*/
    /* if UIFCPY not present Overflow counter can not be safely used -> limitation to 1 OF. */
#ifndef TIM_CNT_UIFCPY
    OverflowCntSample = ( CntCapture < pHandle->PreviousCapture ) ? 1 : 0;//当前值小于上一次的值则为1，否则为0，注意此宏是notdefine
#endif
    //CountDirectionUp 0 ~ 16384-0 ~ 16384, OverflowCntSample == 1说明经过了16384~0的跳变点
    pHandle->DeltaCapturesBuffer[pHandle->DeltaCapturesIndex] =
      ( int32_t )( CntCapture ) - ( int32_t )( pHandle->PreviousCapture ) +
      ( ( int32_t )( OverflowCntSample ) + OFbit ) * ( int32_t )( pHandle->PulseNumber );
  }


  /*Computes & returns average mechanical speed */
  for ( bBufferIndex = 0u; bBufferIndex < bBufferSize; bBufferIndex++ )//bBufferSize = ENC_AVERAGING_FIFO_DEPTH = 16
  {
    wOverallAngleVariation += pHandle->DeltaCapturesBuffer[bBufferIndex];
  }
  //SpeedSamplingFreqUnit = MediumFrequenceTaskRate = 1000 中频任务频率1mS rad/s = (count/16384)/1mS = count * 1000 / 16384
  //此处放大了10倍的转速，count * 10000 / 16384
  wtemp1 = wOverallAngleVariation * ( int32_t )( pHandle->SpeedSamplingFreqUnit );//乘以10000，放大10000倍
  wtemp2 = ( int32_t )( pHandle->PulseNumber ) *
           ( int32_t )( pHandle->SpeedBufferSize );//总的差值个数* 每圈的count数 16384*16
  wtemp1 /= wtemp2;//得到平均速度 (SumOfDeltaCaptureCount * CaptureTime)/16384*16 = (averDeltaCaptureCount/16384)*10000 = avrDeltaCapCount * 0.61
  *pMecSpeedUnit = ( int16_t )( wtemp1 );//得到速度，换算为16位，注意速度是指针,机械速度与实际rad/s的倍数为6，实际机械速度的范围(0~3000*1.15)/6 = 0~575

  /*Computes & stores average mechanical acceleration */
  pHandle->_Super.hMecAccelUnitP = ( int16_t )( wtemp1 -
                                   pHandle->_Super.hAvrMecSpeedUnit );//得到机械加速度，两次机械速度的差值

  /*Stores average mechanical speed */
  pHandle->_Super.hAvrMecSpeedUnit = ( int16_t )wtemp1;//本次速度存入

  /*Computes & stores the instantaneous electrical speed [dpp], var wtemp1*/
  wtemp1 = pHandle->DeltaCapturesBuffer[pHandle->DeltaCapturesIndex] *
           ( int32_t )( pHandle->SpeedSamplingFreqHz ) *
           ( int32_t )pHandle->_Super.bElToMecRatio;//计算瞬时电速度 = 瞬时机械速度 * 极对数

  wtemp1 /= ( int32_t )( pHandle->PulseNumber );//DeltaCount * 1000 / 16384 * 5
  wtemp1 *= ( int32_t )( pHandle->_Super.DPPConvFactor);//(65536/1),瞬时速度扩大65536倍
  wtemp1 /= ( int32_t )( pHandle->_Super.hMeasurementFrequency );//16000 瞬时速度再缩小16000

  pHandle->_Super.hElSpeedDpp = ( int16_t )wtemp1;//存入电瞬时速度，从计算上来看应当是机械瞬时速度的2.048倍，在极对数为5的情况下。
                                                  //瞬时机械速度 deltaCount / 16384 * 10000; 
                                                  //瞬时电速度   deltaCount / 16384 * 1000 * 5 * 65536 / 16000
                                                  //ELE_INSTANTANEOUS_SPEED = MEC_SPEED * 2.048
                                                  //注意瞬时速度不同于电子速度，电子速度与机械速度之比为极对数

  /*last captured value update*/
  pHandle->PreviousCapture = CntCapture;//存入本次定时器的值
  /*Buffer index update*/
  pHandle->DeltaCapturesIndex++;//缓存BUFFER索引递增

  if ( pHandle->DeltaCapturesIndex >= pHandle->SpeedBufferSize )//环形
  {
    pHandle->DeltaCapturesIndex = 0u;
  }

  /*Checks the reliability status, then stores and returns it*/
  if ( pHandle->TimerOverflowError )//如果发生了定时器溢出两次就不可用，理论上每次溢出都会在中等任务中给消除掉溢出标志，不出现两个溢出才执行一次中等任务。
  {
    bReliability = false;
    pHandle->SensorIsReliable = false;
    pHandle->_Super.bSpeedErrorNumber = pHandle->_Super.bMaximumSpeedErrorsNumber;//MEAS_ERRORS_BEFORE_FAULTS = 3

  }
  else
  {
    //通过判断速度，加速度是否符合设定的要求来判定是否可信
    bReliability = SPD_IsMecSpeedReliable( &pHandle->_Super, pMecSpeedUnit );//速度指针入函数
  }

  return ( bReliability );
}

/**
  * @brief  It set instantaneous rotor mechanical angle.
  *         As a consequence, timer counted is computed and updated.
  * @param  pHandle: handler of the current instance of the encoder component
  * @param  hMecAngle new value of rotor mechanical angle (s16degrees)
  * @retval none
  */
__weak void ENC_SetMecAngle( ENCODER_Handle_t * pHandle, int16_t hMecAngle )
{
  TIM_TypeDef * TIMx = pHandle->TIMx;

  uint16_t hAngleCounts;
  uint16_t hMecAngleuint;

  pHandle->_Super.hMecAngle = hMecAngle;//-32768~32767
  pHandle->_Super.hElAngle = hMecAngle * pHandle->_Super.bElToMecRatio;//乘以极对数
  if ( hMecAngle < 0 )//负半圈
  {
    hMecAngle *= -1;//取反
    hMecAngleuint = 65535u - ( uint16_t )hMecAngle;//归一化-32768~0 ==> 32767 ~ 65535
  }
  else
  {
    hMecAngleuint = ( uint16_t )hMecAngle;//0~32767，把输入的值变成hMecAngle的一条直线
  }

  hAngleCounts = ( uint16_t )( ( ( uint32_t )hMecAngleuint *
                                 ( uint32_t )pHandle->PulseNumber ) / 65535u );//转换为定时器的脉冲值，一圈脉冲为PluseNumber 16384

  TIMx->CNT = ( uint16_t )( hAngleCounts );//设定当前定时器捕获的脉冲值
  
}

/**
  * @brief  IRQ implementation of the TIMER ENCODER
  * @param  pHandle: handler of the current instance of the encoder component
  * @param  flag used to distinguish between various IRQ sources
  * @retval none
  */
__weak void * ENC_IRQHandler( void * pHandleVoid )
{
  ENCODER_Handle_t * pHandle = ( ENCODER_Handle_t * ) pHandleVoid;

  /*Updates the number of overflows occurred*/
  /* the handling of overflow error is done in ENC_CalcAvrgMecSpeedUnit */
  pHandle->TimerOverflowNb += 1u;//每次溢出+1，但会在中频任务1mS中置0，如果中频任务中此值大于1则编码器可靠性为false

  return MC_NULL;
}
/**
  * @}
  */

/**
  * @}
  */

/** @} */


/******************* (C) COPYRIGHT 2019 STMicroelectronics *****END OF FILE****/
